Multi-Query Feedback Motion Planning with LQR-Roadmaps

Here we present an algorithm which addresses the need to produce high performance, provably stable feedback controllers for constrained nonlinear systems with goals and constraints that are not fully specified until runtime. Our approach is to precompute a multi-query directed “roadmap”, with each segment representing a locally optimal trajectory of the system and a continuous family of verified finite-time invariant sets, or “funnels”, associated with an LQR controller computed for the trajectory. The result is a library of parameterized local feedback skills which can be efficiently assembled into a provably stable feedback controller at runtime that takes the system from any point in a bounded region in state space to any stabilizable goal state. The paper makes a number of technical contributions, including formulations for exact algebraic verification using Sums-of-Squares optimization of Lyapunov stability for sets of stabilizable goal states exploiting the state-dependent riccati equations (SDRE) and for parameterized finite-time invariance around a trajectory, as well as a “roadmap” construction algorithm that provides probabilistic feedback coverage for any runtime goal in the stabilizable set of the system. We demonstrate our approach with a number of numerical examples.